#include "BoundingSphere.h"
#include "BoundingBox.h"

#include <glm/gtx/norm.hpp>
#include "Frustum.h"

BoundingSphere::BoundingSphere()
{
	mCenter = glm::vec3(0.0f);
	mRadius = 0.0f;
}

BoundingSphere::BoundingSphere(const glm::vec3& center, float radius)
{
	mCenter = center;
	mRadius = radius;
}

BoundingSphere::BoundingSphere(const BoundingSphere& copy)
{
	mCenter = copy.mCenter;
	mRadius = copy.mRadius;
}

BoundingSphere::BoundingSphere(const glm::vec3* points, unsigned int count)
{
	if (count == 0)
	{
		mCenter = glm::vec3(0.0f);
		mRadius = 0.0f;
		return;
	}

	mCenter = glm::vec3(0.0f);
	mRadius = 0.0f;

	for (unsigned int i = 0; i < count; i++)
	{
		mCenter += points[i];
	}

	mCenter /= (float)count;

	float maxDist = 0.0f;
	for (unsigned int i = 0; i < count; i++)
	{
		float dist = glm::distance(points[i], mCenter);
		if (dist > maxDist)
		{
			maxDist = dist;
		}
	}

	mRadius = maxDist;
}

BoundingSphere::BoundingSphere(const glm::vec3* points, unsigned int count, const glm::vec3& center)
{
	if (count == 0)
	{
		mCenter = glm::vec3(0.0f);
		mRadius = 0.0f;
		return;
	}

	mCenter = center;
	mRadius = 0.0f;

	for (unsigned int i = 0; i < count; i++)
	{
		mCenter += points[i];
	}

	mCenter /= (float)count;

	float maxDistSq = 0.0f;
	for (unsigned int i = 0; i < count; i++)
	{
		float dist = glm::length(points[i] - mCenter);
		if (dist > maxDistSq)
		{
			maxDistSq = dist;
		}
	}

	mRadius = maxDistSq;
}

BoundingSphere::BoundingSphere(const glm::vec3* points, unsigned int count, const glm::vec3& center, float radius)
{
	if (count == 0)
	{
		mCenter = glm::vec3(0.0f);
		mRadius = 0.0f;
		return;
	}

	mCenter = center;
	mRadius = radius;

	for (unsigned int i = 0; i < count; i++)
	{
		mCenter += points[i];
	}

	mCenter /= (float)count;

	float maxDistSq = 0.0f;
	for (unsigned int i = 0; i < count; i++)
	{
		float dist = glm::length(points[i] - mCenter);
		if (dist > maxDistSq)
		{
			maxDistSq = dist;
		}
	}

	mRadius = maxDistSq;
}

BoundingSphere& BoundingSphere::operator=(const BoundingSphere& rhs)
{
	if (this == &rhs)
		return *this;

	mCenter = rhs.mCenter;
	mRadius = rhs.mRadius;

	return *this;
}

bool BoundingSphere::IsInside(const glm::vec3& point) const
{
	return glm::length2(point - mCenter) <= mRadius * mRadius;
}

bool BoundingSphere::IsInside(const BoundingSphere& sphere) const
{
	return glm::length2(sphere.mCenter - mCenter) <= (mRadius + sphere.mRadius) * (mRadius + sphere.mRadius);
}

bool BoundingSphere::IsInside(const BoundingBox& box) const
{
	return box.IsInside(*this);
}

bool BoundingSphere::IsInside(const Frustum& frustum) const
{
	return frustum.IsInside(*this);
}

const glm::vec3& BoundingSphere::GetCenter() const
{
	return mCenter;
}

float BoundingSphere::GetRadius() const
{
	return mRadius;
}

void BoundingSphere::SetCenter(const glm::vec3& center)
{
	mCenter = center;
}

void BoundingSphere::SetRadius(float radius)
{
	mRadius = radius;
}

bool BoundingSphere::Contains(const glm::vec3& point) const
{
	return glm::length2(point - mCenter) <= mRadius * mRadius;
}

bool BoundingSphere::Contains(const BoundingSphere& other) const
{
	return glm::distance2(other.mCenter, mCenter) <= (mRadius + other.mRadius) * (mRadius + other.mRadius);
}
bool BoundingSphere::Intersects(const BoundingSphere& other) const
{
	return glm::distance2(other.mCenter, mCenter) <= (mRadius + other.mRadius) * (mRadius + other.mRadius);
}
bool BoundingSphere::Intersects(const glm::vec3& point) const
{
	return glm::distance2(point, mCenter) <= mRadius * mRadius;
}
bool BoundingSphere::Intersects(const glm::vec3& point, float radius) const
{
	return glm::distance2(point, mCenter) <= (mRadius + radius) * (mRadius + radius);
}

void BoundingSphere::Merge(const BoundingSphere& other)
{
	float distance = glm::distance(other.mCenter, mCenter);

	if (distance > mRadius + other.mRadius)
		return;

	if (distance <= mRadius - other.mRadius)
	{
		mCenter = other.mCenter;
		mRadius = other.mRadius;
		return;
	}

	if (distance <= other.mRadius - mRadius)
		return;

	float half = (distance + mRadius + other.mRadius) * 0.5f;
	float scale = half / distance;
	mCenter = (mCenter + other.mCenter) * scale;
	mRadius = half;
}

void BoundingSphere::Merge(const glm::vec3& point)
{
	float distance = glm::distance(point, mCenter);

	if (distance > mRadius)
		return;

	if (distance <= 0.0f)
	{
		mCenter = point;
		mRadius = 0.0f;
		return;
	}

	float half = (distance + mRadius) * 0.5f;
	float scale = half / distance;
	mCenter = (mCenter + point) * scale;
	mRadius = half;
}

void BoundingSphere::Merge(const glm::vec3* points, unsigned int count)
{
	if (count == 0)
		return;

	float radius = 0.0f;
	glm::vec3 center = points[0];

	for (unsigned int i = 1; i < count; i++)
	{
		float distance = glm::distance(points[i], center);

		if (distance > radius)
			radius = distance;

		center += points[i];
	}

	center /= (float)count;

	float distance = glm::distance(center, mCenter);

	if (distance > mRadius)
		return;

	if (distance <= 0.0f)
	{
		mCenter = center;
		mRadius = 0.0f;
		return;
	}

	float half = (distance + mRadius + radius) * 0.5f;
	float scale = half / distance;
	mCenter = (mCenter + center) * scale;
	mRadius = half;
}

void BoundingSphere::Transform(const glm::mat4& transform)
{
	glm::vec3 center = transform * glm::vec4(mCenter, 1.0f);

	mCenter = center;
}
